<?php

// Levenberg-Marquardt in PHP

// http://www.idiom.com/~zilla/Computer/Javanumeric/LM.java

class LevenbergMarquardt {

    /**
     * Calculate the current sum-squared-error
     *
     * Chi-squared is the distribution of squared Gaussian errors,
     * thus the name.
     *
     * @param double[][] $x
     * @param double[] $a
     * @param double[] $y,
     * @param double[] $s,
     * @param object $f
     */
    function chiSquared($x, $a, $y, $s, $f) {
        $npts = count($y);
        $sum = 0.0;

        for ($i = 0; $i < $npts; ++$i) {
            $d = $y[$i] - $f->val($x[$i], $a);
            $d = $d / $s[$i];
            $sum = $sum + ($d*$d);
        }

        return $sum;
    }    //    function chiSquared()


    /**
     * Minimize E = sum {(y[k] - f(x[k],a)) / s[k]}^2
     * The individual errors are optionally scaled by s[k].
     * Note that LMfunc implements the value and gradient of f(x,a),
     * NOT the value and gradient of E with respect to a!
     *
     * @param x array of domain points, each may be multidimensional
     * @param y corresponding array of values
     * @param a the parameters/state of the model
     * @param vary false to indicate the corresponding a[k] is to be held fixed
     * @param s2 sigma^2 for point i
     * @param lambda blend between steepest descent (lambda high) and
     *    jump to bottom of quadratic (lambda zero).
     *     Start with 0.001.
     * @param termepsilon termination accuracy (0.01)
     * @param maxiter    stop and return after this many iterations if not done
     * @param verbose    set to zero (no prints), 1, 2
     *
     * @return the new lambda for future iterations.
     *  Can use this and maxiter to interleave the LM descent with some other
     *  task, setting maxiter to something small.
     */
    function solve($x, $a, $y, $s, $vary, $f, $lambda, $termepsilon, $maxiter, $verbose) {
        $npts = count($y);
        $nparm = count($a);

        if ($verbose > 0) {
            print("solve x[".count($x)."][".count($x[0])."]");
            print(" a[".count($a)."]");
            println(" y[".count(length)."]");
        }

        $e0 = $this->chiSquared($x, $a, $y, $s, $f);

        //double lambda = 0.001;
        $done = false;

        // g = gradient, H = hessian, d = step to minimum
        // H d = -g, solve for d
        $H = array();
        $g = array();

        //double[] d = new double[nparm];

        $oos2 = array();

        for($i = 0; $i < $npts; ++$i) {
            $oos2[$i] = 1./($s[$i]*$s[$i]);
        }
        $iter = 0;
        $term = 0;    // termination count test

        do {
            ++$iter;

            // hessian approximation
            for( $r = 0; $r < $nparm; ++$r) {
                for( $c = 0; $c < $nparm; ++$c) {
                    for( $i = 0; $i < $npts; ++$i) {
                        if ($i == 0) $H[$r][$c] = 0.;
                        $xi = $x[$i];
                        $H[$r][$c] += ($oos2[$i] * $f->grad($xi, $a, $r) * $f->grad($xi, $a, $c));
                    }  //npts
                } //c
            } //r

            // boost diagonal towards gradient descent
            for( $r = 0; $r < $nparm; ++$r)
                $H[$r][$r] *= (1. + $lambda);

            // gradient
            for( $r = 0; $r < $nparm; ++$r) {
                for( $i = 0; $i < $npts; ++$i) {
                    if ($i == 0) $g[$r] = 0.;
                    $xi = $x[$i];
                    $g[$r] += ($oos2[$i] * ($y[$i]-$f->val($xi,$a)) * $f->grad($xi, $a, $r));
                }
            } //npts

            // scale (for consistency with NR, not necessary)
            if ($false) {
                for( $r = 0; $r < $nparm; ++$r) {
                    $g[$r] = -0.5 * $g[$r];
                    for( $c = 0; $c < $nparm; ++$c) {
                        $H[$r][$c] *= 0.5;
                    }
                }
            }

            // solve H d = -g, evaluate error at new location
            //double[] d = DoubleMatrix.solve(H, g);
//            double[] d = (new Matrix(H)).lu().solve(new Matrix(g, nparm)).getRowPackedCopy();
            //double[] na = DoubleVector.add(a, d);
//            double[] na = (new Matrix(a, nparm)).plus(new Matrix(d, nparm)).getRowPackedCopy();
//            double e1 = chiSquared(x, na, y, s, f);

//            if (verbose > 0) {
//                System.out.println("\n\niteration "+iter+" lambda = "+lambda);
//                System.out.print("a = ");
//                (new Matrix(a, nparm)).print(10, 2);
//                if (verbose > 1) {
//                    System.out.print("H = ");
//                    (new Matrix(H)).print(10, 2);
//                    System.out.print("g = ");
//                    (new Matrix(g, nparm)).print(10, 2);
//                    System.out.print("d = ");
//                    (new Matrix(d, nparm)).print(10, 2);
//                }
//                System.out.print("e0 = " + e0 + ": ");
//                System.out.print("moved from ");
//                (new Matrix(a, nparm)).print(10, 2);
//                System.out.print("e1 = " + e1 + ": ");
//                if (e1 < e0) {
//                    System.out.print("to ");
//                    (new Matrix(na, nparm)).print(10, 2);
//                } else {
//                    System.out.println("move rejected");
//                }
//            }

            // termination test (slightly different than NR)
//            if (Math.abs(e1-e0) > termepsilon) {
//                term = 0;
//            } else {
//                term++;
//                if (term == 4) {
//                    System.out.println("terminating after " + iter + " iterations");
//                    done = true;
//                }
//            }
//            if (iter >= maxiter) done = true;

            // in the C++ version, found that changing this to e1 >= e0
            // was not a good idea.  See comment there.
            //
//            if (e1 > e0 || Double.isNaN(e1)) { // new location worse than before
//                lambda *= 10.;
//            } else {        // new location better, accept new parameters
//                lambda *= 0.1;
//                e0 = e1;
//                // simply assigning a = na will not get results copied back to caller
//                for( int i = 0; i < nparm; i++ ) {
//                    if (vary[i]) a[i] = na[i];
//                }
//            }
        } while(!$done);

        return $lambda;
    }    //    function solve()

}    //    class LevenbergMarquardt
